/* The is the Rover Team Project Code for The California Institute of Technology 2012.
Copyright (C) 2012 scchang, bryanhe, dflicker

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */
#ifndef INPUT_READER_
#define INPUT_READER_

#include "include/input_reader.h"

InputReader::InputReader(const char* fd_joystick, const char* fd_ardui, int ardui_baud){
  printf("Atttempting to init joystick\n");
  this->joystick = new Joystick(fd_joystick);
  printf("Sucess at init joystick\n");
  printf("Attempting to init serial comm.\n");
  this->ardui = new SerialComm(fd_ardui, ardui_baud);
  printf("Success at init seria_comm.\n");
  sleep(2);

  unsigned char* toSend = new unsigned char[1];
  toSend[0] = '#';
  if(this->ardui->sendString(toSend, 1) != SUCCESS_COMM){
    fprintf(stderr, "In InputReader::getArduino, unable to send string.\n");
  }

  unsigned char* buff;
  if(this->ardui->readString(&buff, '$') == SUCCESS_COMM){
    printf("%s\n", buff);
    delete[] buff;
  }
  sleep(2);

  /*while(true){
    unsigned char* toSend = new unsigned char[1];
    toSend[0] = '#';
    if(this->ardui->sendString(toSend, 1) != SUCCESS_COMM){
      fprintf(stderr, "In InputReader::getArduino, unable to send string.\n");
    }

    unsigned char* buff;
    if(this->ardui->readString(&buff, '$') == SUCCESS_COMM){
      printf("%s\n", buff);
      delete[] buff;
    }
    sleep(2);
    }*/
}

InputReader::~InputReader(void){
  delete(this->joystick);
}

int InputReader::getInputState(InputState** state){
  *state = new InputState();
  if(getJoystick(*state) < 0){
    fprintf(stderr, "In InputReader::getInputState, unable to read joysticks.\n");
    return -1;
  }
  if(getArduino(*state) < 0){
    fprintf(stderr, "In InputReader::getInputState, uanble to read arduino.\n");
    return -1;
  }
  return 0;
}

int InputReader::getJoystick(InputState* state){
  int numAxes = this->joystick->getNumAxes();
  int numButs = this->joystick->getNumButtons();

#ifdef DEBUG
  printf("InputReader::getJoystick: \n");
  printf("Num Axis: %d\n", numAxes);
  printf("Num Buts: %d\n", numButs);
#endif

  // First Axis Into State
  // Negate to make right 255 and left 1
  float percent = -(((float)this->joystick->getAxis(JOYSTICK_X_AXIS))/JOYSTICK_MAX);
  int adding = 127 * percent;
  unsigned char final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_X, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set joystick x.\n");
    return -1;
  }

  percent = (((float)this->joystick->getAxis(JOYSTICK_Y_AXIS))/JOYSTICK_MAX);
  adding = 127 * percent;
  final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_Y, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set joystick y.\n");
    return -1;
  }

  percent = (((float)this->joystick->getAxis(JOYSTICK_TWI_AXIS))/JOYSTICK_MAX);
  adding = 127 * percent;
  final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_TWI, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, uanble to set joystick twist.\n");
    return -1;
  }
  
  percent = (((float)this->joystick->getAxis(JOYSTICK_TH_X_AXIS))/JOYSTICK_MAX);
  adding = 127 * percent;
  final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_TH_X, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, uanble to set joystick th x.\n");
    return -1;
  }

  percent = (((float)this->joystick->getAxis(JOYSTICK_TH_Y_AXIS))/JOYSTICK_MAX);
  adding = 127 * percent;
  final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_TH_Y, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set joystick th y.\n");
    return -1;
  }
  
  percent = (((float)this->joystick->getAxis(JOYSTICK_THR_AXIS))/JOYSTICK_MAX);
  adding = 127 * percent;
  final = (unsigned char)(128 + adding);
  if(state->setValue(JOY_THR, final) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set joystick thr y.\n");
    return -1;
  }

  // Now get buttons
  int button0 = 0;
  int button1 = 0;
  int value = 1;
  for(int i = 0; i < 8; ++i, value*=2){
    if((int)this->joystick->getButton(i) == 1){
      button0 += value;
    }
  }

  value = 1;
  for(int i = 0; i < 4; ++i, value*=2){
    if((int)this->joystick->getButton(i+8) == 1){
      button1 += value;
    }
  }
  
  if(state->setValue(JOY_BUT_0, (unsigned char)button0) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set button 0.\n");
    return -1;
  }

  if(state->setValue(JOY_BUT_1, (unsigned char)button1) < 0){
    fprintf(stderr, "In InputReader::getJoystick, unable to set button 1.\n");
    return -1;
  }
  return 0;
}

int InputReader::getArduino(InputState* state){
  unsigned char* toSend = new unsigned char[1];
  toSend[0] = '#';
  if(this->ardui->sendString(toSend, 1) != SUCCESS_COMM){
    fprintf(stderr, "In InputReader::getArduino, unable to send string.\n");
    return -1;
  }

  unsigned char* buff;
  if(this->ardui->readString(&buff, '$') == SUCCESS_COMM){
    printf("%s\n", buff);
  }else{
    fprintf(stderr, "In InputReader::getARduino, unable to recieve string.\n");
    fprintf(stderr, "Setting defaults\n");
    unsigned char def = (unsigned char)128;
    state->setValue(MINI_SHOUL_PAN, def);
    state->setValue(MINI_SHOUL_ELE, def);
    state->setValue(MINI_ELBOW, def);
    state->setValue(MINI_WRIST, def);
    state->setValue(MINI_TWIST, def);
    state->setValue(MINI_GRIP, def);
    state->setValue(BUTTONS, 0);
    return -1;
  }

  unsigned char values[7];

  int index = 0;
  while(buff[index] != '~'){
    index++;
  }
  index++;

  // Outsouce later
  for(int i = 0; i < 7; ++i){
    std::vector<char> vec;
    int j = 0;
    for(; buff[index+j] != '$' && buff[index+j] != '#'; ++j){
      vec.push_back(buff[index+j]);
    }
    char* temp = new char[vec.size()];
    for(int k = 0; k < j; k++){
      temp[k] = vec[k];
    }
    
    int intForm = atoi(temp);
    printf("%d ", intForm);
    delete[] temp;

    values[i] = (unsigned char)intForm;
    index += j + 1;
  }
  printf("\n");

  
  state->setValue(MINI_SHOUL_PAN, values[0]);
  state->setValue(MINI_SHOUL_ELE, values[1]);
  state->setValue(MINI_ELBOW, values[2]);
  state->setValue(MINI_WRIST, values[3]);
  state->setValue(MINI_TWIST, values[4]);
  state->setValue(BUTTONS, values[6] - 1);
  //  state->setValue(MINI_TWIST, values[5]);
  delete[] buff;

  return 0;
}

#endif
